/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.macro;

import com.googlecode.gunncsnavigation.telemetry.TelemetryPorts;
import com.grt192.actuator.spot.GRTDemoLED;
import com.grt192.controller.spot.SensorServer;
import com.grt192.core.GRTObject;
import com.grt192.core.controller.Macro;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.event.component.spot.PulseSwitchListener;
import com.grt192.mechanism.spot.MaxbotixSoftConstantLoop;
import com.grt192.mechanism.spot.SLight;
import com.grt192.rsh.Commandable;
import com.grt192.rsh.RSHServer;
import com.grt192.sensor.spot.GRTPulseSwitch;
import com.grt192.sensor.spot.GRTSonar;
import com.grt192.utils.Util;
import com.sun.spot.resources.transducers.IIOPin;
import com.sun.spot.sensorboard.EDemoBoard;

/**
 * 
 * @author ajc
 */
public class SonarSpot extends GRTObject implements Macro, TelemetryPorts,
		SensorChangeListener, PulseSwitchListener, Commandable {

	public SonarSpot() {
		setPrinting(true);
		RSHServer.getInstance().addListener(this);
		RSHServer.getInstance().setPrinting(true);
	}

	public int execute() {
		initLights();
		startSensors();
		startListening();
		while (true) {
			Util.sleep(1000);
		}
	}

	private SLight[] lights = SLight.get();
	private int[] sonarLightIDs = new int[] { 2, 3, 4 };
	private int txLightID = 5;

	public void initLights() {
		// sonars
		for (int i = 0; i < sonarLightIDs.length; i++) {
			lights[sonarLightIDs[i]].rawColor(GRTDemoLED.Color.ORANGE);
			lights[sonarLightIDs[i]].blinkToBlack();
		}
		// tx pin
		lights[txLightID].rawColor(GRTDemoLED.Color.WHITE);
	}

	private static final int NUM_SONARS = 3;
	private static final int RX_PIN = 4;
	private static final int TX_PIN = 0;
	GRTSonar[] sonars;
	IIOPin rx;
	GRTPulseSwitch tx;
	MaxbotixSoftConstantLoop mscl;
	SensorServer[] servers;

	public void startSensors() {
		log("Starting sonars...");
		sonars = new GRTSonar[NUM_SONARS];
		for (int i = 0; i < sonars.length; i++) {
			sonars[i] = GRTSonar.fromAnalog(i, 50, "Sonar" + i);
			// sonars[i].setChangeThreshold(txLightID);
			sonars[i].setChangeThreshold(1);
			sonars[i].start();
			log("\t" + sonars[i].getID() + "started");
		}
		log("Setting rx " + RX_PIN + " low...");
		rx = EDemoBoard.getInstance().getIOPins()[RX_PIN];
		rx.setAsOutput(true);
		rx.setLow();

		log("Starting pulseswitch " + TX_PIN + " ...");
		tx = new GRTPulseSwitch(TX_PIN, 5, "d" + TX_PIN);
		// tx.setPrior
		tx.addPulseSwitchListener(this);
		tx.start();

		log("Starting MaxbotixDaisyChain...");
		mscl = new MaxbotixSoftConstantLoop(sonars, rx, tx);
		mscl.startRanging();

		log("Starting Telemetry servers...");
		servers = new SensorServer[NUM_SONARS];
		for (int i = 0; i < servers.length; i++) {
			servers[i] = new SensorServer(sonars[i], SONAR_PORTS[i]);
			servers[i].start();
			log("\t new TelemetryServer hosting " + sonars[i].getID()
					+ " on port " + SONAR_PORTS[i]);
		}

	}

	public void startListening() {
		log("Listening to sonars...");
		for (int i = 0; i < sonars.length; i++) {
			sonars[i].addSensorChangeListener(this);
			log("\t listening to " + sonars[i].getID());
		}
	}

	public void sensorStateChanged(SensorEvent e, String key) {
		for (int i = 0; i < sonars.length; i++) {
			if (sonars[i] == e.getSource()) {
				lights[sonarLightIDs[i]].blinkToBlack();
				break;
			}
		}
		log("distance: " + e.getSource().getID() + ": " + e.getData(key));
	}

	public void pulseRead(GRTPulseSwitch s, int usec) {
		lights[txLightID].blinkToBlack();
		log("Pulse read for " + usec + " usecs");
	}

	public void command(String[] args) {
		log("SonarSpot commanded: " + Util.arraytoString(args));
		// mscl.star
	}
}
